#include "Sample.h"

#undef max
#undef min

Sample::Sample(float missRate, int frameGap) 
{
	this->missRate = missRate;
	this->frameGap = frameGap;
	this->Pinit_term = pow(missRate*1.0, sqrt(frameGap*1.0));
}

Sample::~Sample() 
{
	positiveSmp.clear();
	negativeSmp.clear();
	featurePool.clear();
	strongClassifier.clear();
	yi.clear();
	index.clear();
	output.clear();
}

void Sample::Init()//Container t)
{
	//this->container = t;
	container.RemoveDuplicateDSet();
}

void Sample::ComputeFeaturePool(CvRect rect1, CvRect rect2, int frame1, int frame2)
{
	//string fileName = "x0";
	char str[3] = "1";

	//itoa(frame1,str,10);
	//fileName.append(str);
	//fileName.append(".jpg");

	//string fileName = "image\\x01 (";
	string fileName = "image1\\x01 (";
	itoa(frame1,str,10);
	fileName.append(str);
	fileName.append(").jpg");
	IplImage *pImg = cvLoadImage((char *)fileName.c_str(), 1);
	IplImage *img1 = cvCreateImage(cvSize(rect1.width,rect1.height), 8, 3);
	cvSetImageROI(pImg, rect1);
	cvCopy(pImg, img1);
	cvResetImageROI(pImg);

	//fileName = "image\\x01 (";
	fileName = "image1\\x01 (";
	itoa(frame2,str,10);
	fileName.append(str);
	fileName.append(").jpg");
	pImg = cvLoadImage((char *)fileName.c_str(), 1);
	IplImage *img2 = cvCreateImage(cvSize(rect2.width,rect2.height), 8, 3);
	cvSetImageROI(pImg, rect2);
	cvCopy(pImg, img2);
	cvResetImageROI(pImg);

	this->ComputeAppDescriptor(img1, img2);

	cvReleaseImage(&img1);
	cvReleaseImage(&img2);
	cvReleaseImage(&pImg);
}

void Sample::ComputeAppDescriptor(IplImage *img1, IplImage *img2)
{
	this->elem.clear();
	Hist* h1 = new Hist(cvGetSize(img1));
	Hist* h2 = new Hist(cvGetSize(img2));
	Utils *utils = new Utils();
	vector<float> mat1;
	vector<float> mat2;

	//vector<float> mat1 = h1->computeHist(img1, cvRect(0,0,img1->width/2,img1->height/2));
	//vector<float> mat2 = h2->computeHist(img2, cvRect(0,0,img2->width/2,img2->height/2));
	//float r = utils->ComputeDistance(mat1, mat2);
	//this->elem.push_back(r);
	//mat1.clear();
	//mat2.clear();

	//mat1 = h1->computeHist(img1, cvRect(img1->width/2, 0, img1->width/2, img1->height/2));
	//mat2 = h2->computeHist(img2, cvRect(img2->width/2, 0, img2->width/2, img2->height/2));
	//r = utils->ComputeDistance(mat1, mat2);
	//this->elem.push_back(r);
	//mat1.clear();
	//mat2.clear();

	//mat1 = h1->computeHist(img1, cvRect(0, img1->height/2, img1->width/2, img1->height/2));
	//mat2 = h2->computeHist(img2, cvRect(0, img2->height/2, img2->width/2, img2->height/2));
	//r = utils->ComputeDistance(mat1, mat2);
	//this->elem.push_back(r);
	//mat1.clear();
	//mat2.clear();

	//mat1 = h1->computeHist(img1, cvRect(img1->width/2, img1->height/2, img1->width/2, img1->height/2));
	//mat2 = h2->computeHist(img2, cvRect(img2->width/2, img2->height/2, img2->width/2, img2->height/2));
	//r = utils->ComputeDistance(mat1, mat2);
	//this->elem.push_back(r);
	//mat1.clear();
	//mat2.clear();

	/* compute HOG features */
	HOG hog;	
	float d;
	int i, width1, width2, height1, height2;
	// divide width into 3
	for (i = 0, d = 0, width1 = img1->width/3, width2 = img2->width/3; i < 3; i++)
	{
		mat1 = h1->computeHist(img1, cvRect(i*width1,0,width1,img1->height));
		mat2 = h2->computeHist(img2, cvRect(i*width2,0,width2,img2->height));
		d = utils->ComputeDistance(mat1, mat2);
		this->elem.push_back(d);
		mat1.clear();
		mat2.clear();

		d = hog.ComputeHOGDistanceImg(img1,cvRect(i*width1,0,width1,img1->height),
			img2,cvRect(i*width2,0,width2,img2->height),8);
		this->elem.push_back(d);		
	}

	// divide height into 5
	for (i = 0, d = 0, height1 = img1->height/5, height2 = img2->height/5; i < 5; i++)
	{
		mat1 = h1->computeHist(img1, cvRect(0,i*height1,img1->width,height1));
		mat2 = h2->computeHist(img2, cvRect(0,i*height2,img2->width,height2));
		d = utils->ComputeDistance(mat1, mat2);
		this->elem.push_back(d);
		mat1.clear();
		mat2.clear();

		d = hog.ComputeHOGDistanceImg(img1,cvRect(0,i*height1,img1->width,height1),
			img2,cvRect(0,i*height2,img2->width,height2),8);
		this->elem.push_back(d);
	}

	//// half above	
	//float d = hog.ComputeHOGDistanceImg(img1,cvRect(0,0,img1->width,img1->height/2),
	//	img2,cvRect(0,0,img2->width,img2->height/2),8);
	//this->elem.push_back(d);

	//// half below	
	//d = hog.ComputeHOGDistanceImg(img1,cvRect(0,img1->height/2,img1->width,img1->height),
	//	img2,cvRect(0,img2->height/2,img2->width,img2->height),8);
	//this->elem.push_back(d);

	//// half left
	//d = hog.ComputeHOGDistanceImg(img1,cvRect(0,0,img1->width/2,img1->height),
	//	img2,cvRect(0,0,img2->width/2,img2->height),8);
	//this->elem.push_back(d);

	//// half right	
	//d = hog.ComputeHOGDistanceImg(img1,cvRect(img1->width/2,0,img1->width,img1->height),
	//	img2,cvRect(img2->width/2,0,img2->width,img2->height),8);
	//this->elem.push_back(d);

	/* compute covariance matrix */
	//CovarianceMatrix cm;
	//CvMat c2 = cm.computeCovarianceMatrix(img2);
	//CvMat c1 = cm.computeCovarianceMatrix(img1);
	//CvMat* eigenValues = cm.computeEigenValues(c1,c2);

	//for (int i = 0; i < eigenValues->rows; i++)
	//{
	//	this->elem.push_back(cvGetReal2D(eigenValues,i,0));
	//}

	this->featurePool.push_back(elem);

	delete utils;
	delete h1;
	delete h2;
}

void Sample::ReSample()
{
	Utils *utils = new Utils();
	int nTracklet = container.tracklets.size();
	int size;
	int dSize;
	int m, n;
	Tracklet t;
	Tracklet d;

	for (int i = 0; i < nTracklet; i++)
	{
		t = container.tracklets[i];

		// positive sample
		size = t.trackletSet.size();
		for (m = 0; m + 2 < size; m+=2)
		{
			n = m + 2;
			int *temp = new int[5];
			temp[0] = i;
			temp[1] = m;
			temp[2] = i;
			temp[3] = n;

			ComputeFeaturePool((t.trackletSet)[m], (t.trackletSet)[n], (t.frame)[m], (t.frame)[n]);
			yi.push_back(1);

			temp[4] = yi.size() - 1;
			positiveSmp.push_back(temp);
			//index.push_back(positiveSmp.size() - 1);
		}
		for (m = 1; m + 2 < size; m+=2)
		{
			n = m + 2;
			int *temp = new int[5];
			temp[0] = i;
			temp[1] = m;
			temp[2] = i;
			temp[3] = n;
			positiveSmp.push_back(temp);

			ComputeFeaturePool((t.trackletSet)[m], (t.trackletSet)[n], (t.frame)[m], (t.frame)[n]);
			yi.push_back(1);

			temp[4] = yi.size() - 1;
			positiveSmp.push_back(temp);
			//index.push_back(positiveSmp.size() - 1);
		}
		if (size == 2)
		{
			int *temp = new int[5];
			temp[0] = i;
			temp[1] = 0;
			temp[2] = i;
			temp[3] = 1;
			positiveSmp.push_back(temp);

			ComputeFeaturePool((t.trackletSet)[0], (t.trackletSet)[1], (t.frame)[0], (t.frame)[1]);
			yi.push_back(1);

			temp[4] = yi.size() - 1;
			positiveSmp.push_back(temp);
			//index.push_back(positiveSmp.size() - 1);
		}



		/*size = t.trackletSet.size();
		for (m = 0; m < size-1; m++)
		{
		for (n = m+1; n < size; n++)
		{
		int *temp = new int[4];
		temp[0] = i;
		temp[1] = m;
		temp[2] = i;
		temp[3] = n;
		positiveSmp.push_back(temp);

		ComputeFeaturePool((t.trackletSet)[m], (t.trackletSet)[n], (t.frame)[m], (t.frame)[n]);
		yi.push_back(1);
		index.push_back(positiveSmp.size() - 1);
		}
		}*/

		// negative sample
		dSize = t.discrmSet.size();
		m = size - 1;
		for (int j = 0; j < dSize; j++)
		{
			int k = (t.discrmSet)[j]; //discriminative set index
			d = container.tracklets[k]; // tracklet set corresponding to index k

			int *temp = new int[5];
			temp[0] = i;
			temp[1] = m;
			temp[2] = k;
			temp[3] = 0;
			negativeSmp.push_back(temp);

			ComputeFeaturePool((t.trackletSet)[m], (d.trackletSet)[0], (t.frame)[m], (d.frame)[0]);
			yi.push_back(-1);

			temp[4] = yi.size() - 1;
			positiveSmp.push_back(temp);
			//index.push_back(negativeSmp.size() - 1);

			/*for (n = 0; n < d.trackletSet.size(); n++)
			{
			int *temp = new int[5];
			temp[0] = i;
			temp[1] = m;
			temp[2] = k;
			temp[3] = n;
			negativeSmp.push_back(temp);

			ComputeFeaturePool((t.trackletSet)[m], (d.trackletSet)[n], (t.frame)[m], (d.frame)[n]);
			yi.push_back(-1);
			index.push_back(negativeSmp.size() - 1);
			}	*/				
		}
	}

	delete utils;
}

void Sample::LearningAppModel()
{
	strongClassifier = featurePool;

	float r = 0;
	int kMin = 0;
	float sum = 0;
	float sumMin = 0;
	int size = featurePool.size(); // number of sample x
	vector<float> w;
	for (int i = 0; i < size; i++)
	{
		if (yi[i] > 0)
		{
			w.push_back(1/ (2.0*positiveSmp.size()));
		} else
		{
			w.push_back(1/ (2.0*negativeSmp.size()));
		}

	}
	//float w1 = 1/ (2.0*positiveSmp.size());
	//float w2 = 1/ (2.0*negativeSmp.size());	

	int nPos = 0;
	vector<float> a;

	for (int t = 0; t < 16; t++)
	{
		for (int k = 0; k < 16; k++)
		{
			r = 0;
			for (int j = 0; j < size; j++)
			{
				if (yi[j] > 0)
				{
					r += w[j] * (featurePool[j])[k];
				}
				else {
					r += w[j] * (-1) * (featurePool[j])[k];
				}			
			}

			a.push_back( 0.5 * log((1+r)/(1-r)) );
		}

		// find k for minimize Z
		kMin = 0;
		for (int j = 0; j < size; j++)
		{
			if (yi[j] > 0)
			{
				sum += w[j] * exp(-1 * a[0] * (featurePool[j])[0]);
			}
			else {
				nPos = j;
				sum += w[j] * exp(a[0] * (featurePool[j])[0]);
			}			
		}
		sumMin = sum;
		for (int k = 1; k < 16; k++)
		{
			sum = 0;
			for (int j = 0; j < size; j++)
			{
				if (yi[j] > 0)
				{
					sum += w[j] * exp(-1 * a[k] * (featurePool[j])[k]);
				}
				else {
					sum += w[j] * exp(a[k] * (featurePool[j])[k]);
				}			
			}
			if (sum < sumMin)
			{
				sumMin = sum;
				kMin = k;
			}
		}

		// set alpha = a(kmin) h = h(kmin)
		alpha.push_back(a[kMin]);
		for (int j = 0; j < size; j++)
		{
			(strongClassifier[j])[t] = (featurePool[j])[kMin];
		}

		// update weight		
		float maxElement = w[0] * exp( -1 * alpha[t] * yi[0] * (strongClassifier[0])[t]);
		for (int j = 0; j < size; j++)
		{			
			w[j] = w[j] * exp( -1 * alpha[t] * yi[j] * (strongClassifier[j])[t]);			
			if (w[j] > maxElement)
			{
				maxElement = w[j];
			}
		}

		// normalize
		for (int j = 0; j < size; j++)
		{
			w[j] = w[j] / maxElement;			
		}

		//w1 = w1 * exp( -1 * alpha[t] * (strongClassifier[0])[t] );
		//w2 = w2 * exp( alpha[t] * (strongClassifier[nPos])[t] );
	}	
	w.clear();
	a.clear();
}

void Sample::ComputeResult()
{
	int size = strongClassifier.size();
	float sum = 0;
	for (int i = 0; i < size; i++)
	{
		sum = 0;
		for (int t = 0; t < 16; t++)
		{
			sum += alpha[t] * (strongClassifier[i])[t];
		}
		output.push_back(sum);
	}

	this->NormalizeH();
}

float Sample::ComputeMotionAffinity(Tracklet tj, Tracklet ti)
{
	CvRect rect_i = ti.trackletSet.back();
	CvRect rect_j = tj.trackletSet.front();
	int deltaT = abs(ti.frame.back() - tj.frame.front());

	if (deltaT == 0) // two detect responds are at the identical frame
	{
		return 0;
	}
	if (deltaT > frameGap)
	{
		return 0;
	}
	CvRect rect = ti.trackletSet.front();
	CvRect rect1 = tj.trackletSet.back();

	float px_tail = rect_i.x + rect_i.width/2;
	float py_tail = rect_i.y + rect_i.height/2;

	float vx_tail, vy_tail;
	if (ti.trackletSet.size() == 1) // tracklet contain just one detect respond => assume velocity
	{
		vx_tail = 3;
		vy_tail = 3;
	}
	else {
		vx_tail = (abs(px_tail - rect.x - rect.width/2)) / ti.trackletSet.size();
		vy_tail = (abs(py_tail - rect.y - rect.height/2)) / ti.trackletSet.size();
	}


	float px_head = rect_j.x + rect_j.width/2;
	float py_head = rect_j.y + rect_j.height/2;
	float vx_head, vy_head;
	if (tj.trackletSet.size() == 1)
	{
		vx_head = 3;
		vy_head = 3;
	}
	else {
		vx_head = (abs(px_head - rect1.x - rect1.width/2)) / tj.trackletSet.size();
		vy_head = (abs(py_head - rect1.y - rect1.height/2)) / tj.trackletSet.size();
	}

	float val_ptail[] = {px_tail, py_tail};
	float val_vtail[] = {vx_tail, vy_tail};
	float val_phead[] = {px_head, py_head};
	float val_vhead[] = {vx_head, vy_head};

	/*float sigma1_tail = vx_tail*deltaT;
	float sigma2_tail = vy_tail*deltaT;
	float sigma1_head = vx_head*deltaT;
	float sigma2_head = vy_head*deltaT;*/

	/*float var1 = Utils::ComputeVariance(sigma1_tail, sigma2_tail);
	float var2 = Utils::ComputeVariance(sigma1_head, sigma2_head);
	float val_sigma[] = {var1, var1};*/

	float val_sigma1[] = {vx_tail*deltaT, vy_tail*deltaT};
	float val_sigma2[] = {vx_head*deltaT, vy_head*deltaT};

	CvMat p_tail, v_tail;
	CvMat p_head, v_head, sigma1, sigma2;
	cvInitMatHeader(&p_tail, 1, 2, CV_32FC1, val_ptail);
	cvInitMatHeader(&v_tail, 1, 2, CV_32FC1, val_vtail);
	cvInitMatHeader(&p_head, 1, 2, CV_32FC1, val_phead);
	cvInitMatHeader(&v_head, 1, 2, CV_32FC1, val_vhead);
	cvInitMatHeader(&sigma1, 1, 2, CV_32FC1, val_sigma1);
	cvInitMatHeader(&sigma2, 1, 2, CV_32FC1, val_sigma2);

	/*val_sigma[0] = var2;
	val_sigma[1] = var2;*/	

	CvMat *X = cvCreateMat(1, 2, CV_32FC1);

	cvScale(&v_tail, &v_tail, deltaT);
	cvAdd(&p_tail, &v_tail, X);
	float G1 = Utils::CalGaussian(X, &p_head, &sigma1);

	cvScale(&v_head, &v_head, deltaT);
	cvSub(&p_head, &v_head, X);
	float G2 = Utils::CalGaussian(X, &p_tail, &sigma2);

	return G1 * G2;
}

float Sample::ComputeTemporalAffinity(int deltaT)
{
	int omega = 0; // number of frames in which object is occluded
	int nframe; // number of frames in which object is visible but missed by detector
	nframe = deltaT - 1 - omega;
	float at = 0; // temporal affinity
	if (deltaT <= frameGap)
	{
		at = (float)pow(missRate, nframe);
	} 
	return 1;
}

double Sample::ComputeTrackletsPlink(int j, int i)
{	
	Tracklet ti = container.tracklets[i];
	Tracklet tj = container.tracklets[j];
	float Am = ComputeMotionAffinity(tj, ti); // compute Am(Tj|Ti)
	int deltaT = abs(ti.frame.back() - tj.frame.front());

	float At = ComputeTemporalAffinity(deltaT);
	float Aa = 1;

	int *sample = new int[5];
	int size = negativeSmp.size();
	for (int k = 0; k < size; k++)
	{
		sample = negativeSmp[k];
		if (sample[0] == i && sample[2] == j) {
			Aa = output[sample[4]];
			break;
		}
	}

	return Aa*Am*At*100000000; // normalized factor of 10000000
}

void Sample::AssociateTracklet()
{
	int n = container.tracklets.size();	
	double* transitionMatrix = (double *)calloc(2*n * 2*n,  sizeof(double));
	//double* transitionMatrix = new double(2*n * 2*n); // God! I don't know why this allocation mess everything
	vector<Tracklet> tracklets = container.tracklets;
	double maxValue = 0;

	// create transition matrix
	printf("Number of tracklets: %d\n", n);
	for (int i = 0; i < 2*n; i++)
	{
		for (int j = 0; j < 2*n; j++)
		{				
			if (i == j && i <= n-1)
			{				
				//transitionMatrix[i + 2*n*j] = log(pow(missRate*1.0, 1.0*tracklets[i].trackletSet.size()));
				transitionMatrix[i + 2*n*j] = -1000;
			}
			else if (i <= n-1 && j <= n-1 && i != j)
			{
				double plink = ComputeTrackletsPlink(j,i);
				//if (plink == 0)
				//{
				//	plink = 0.0000000001;
				//}
				//if (plink == std::numeric_limits<double>::infinity())
				//{
				//	plink = std::numeric_limits<float>::max();
				//}

				printf("Use Plink=%e ",plink);
				transitionMatrix[i + 2*n*j] = log(plink) + 
					0.5*(log(pow((1-missRate)*1.0, 1.0*tracklets[i].trackletSet.size())) + 
					log(pow((1-missRate)*1.0, 1.0*tracklets[j].trackletSet.size())));
			}
			else if (i == j + n)
			{
				/*transitionMatrix[i + 2*n*j] = log(Pinit_term) + 
					0.5*log(pow((1-missRate)*1.0, 1.0*tracklets[j].trackletSet.size()));*/
				transitionMatrix[i + 2*n*j] = - 900;
			}
			else if (j == i + n)
			{
				/*transitionMatrix[i + 2*n*j] = log(Pinit_term) + 
					0.5*log(pow((1-missRate)*1.0, 1.0*tracklets[i].trackletSet.size()));*/
				transitionMatrix[i + 2*n*j] = - 900;
			}
			else if (i > n-1 && j > n-1)
			{
				transitionMatrix[i + 2*n*j] = -700;
			}
			else
			{
				transitionMatrix[i + 2*n*j] = -std::numeric_limits<double>::infinity();
			}

			if (maxValue < transitionMatrix[i+2*n*j])
			{
				maxValue = transitionMatrix[i+2*n*j];
			}
			printf("C(%d,%d): %f\n",i,j,transitionMatrix[i + 2*n*j]);
		}
	}

	//FILE *pFile;
	//pFile = fopen("aa.txt","w");

	// turn into max assignment problem
	for (int i = 0; i < 2*n * 2*n; i++)
	{		
		transitionMatrix[i] = maxValue - transitionMatrix[i];
		//printf("%f ",transitionMatrix[i]);
		//if (i % 10 == 0 && i != 0)
		//{
		//	printf("\n");
		//}		
		//fprintf(pFile, "%f\t", transitionMatrix[i]);
	}
	//fclose(pFile);

	// match tracklets using assignment algorithm
	int* assignment = (int *)calloc(2*n,  sizeof(int));
	//int* assignment = new int(2*n);
	double *cost = new double();	

	Hungarian* hungarian = new Hungarian();
	hungarian->assignmentoptimal(assignment, cost, transitionMatrix, 2*n,2*n);
	printf("Assignment result:\n");
	for (int i = 0; i < 10; i++)
	{
		printf("%d\n",assignment[i]);
	}
	//printf("%f\n",*cost);
	free(transitionMatrix);	
	delete hungarian;
	delete cost;

	// matching tracklets
	vector<vector<int>> trackletGroup;
	for (int i = 0; i < 2*n; i++)
	{
		int j = assignment[i];
		if (i == j && i <= n-1)
		{
			// false alarm --> delete the tracklet			
			//container.tracklets.erase(container.tracklets.begin() + i);
		} else if (i <= n-1 && j <= n-1 && i != j)		
		{
			// link the tail of Ti to head of Tj
			if (tracklets[i].frame.back() >= tracklets[j].frame.front())
			{
				continue;	// iTail's frame must smaller then jHead's frame
			}	
			int iIndex = isTraversed(i, trackletGroup);
			int jIndex = isTraversed(j, trackletGroup);
			if (iIndex != -1)	// i already traversed
			{				
				if (jIndex == -1)	// j not traversed
				{
					// add j to group of i
					trackletGroup[iIndex].push_back(j);
				}
				else if (jIndex == iIndex) // i-j already traversed
				{
					continue;
				}
				else
				{
					// merge 2 groups containing i and j
					trackletGroup[iIndex].insert(trackletGroup[iIndex].end(),
						trackletGroup[jIndex].begin(), trackletGroup[jIndex].end());
					trackletGroup.erase(trackletGroup.begin() + jIndex);
				}
			} else if (jIndex != -1) // j already traversed
			{
				// add i to group of j
				trackletGroup[jIndex].push_back(i);
			}
			else
			{
				// create new tracklet
				vector<int> newGroup;
				newGroup.push_back(i);
				newGroup.push_back(j);
				trackletGroup.push_back(newGroup);
			}
		} else if (i == j + n)		
		{
			// Tj is the head of generated trajectory
		} else if (i + n == j)
		{
			// Ti is the tail of generated trajectory
		}
	}

	// associate tracklets
	vector<Tracklet> newTracklets;
	vector<int> trackletToRemove;
	int label = 0;
	for (int i = 0; i < trackletGroup.size(); i++)
	{
		sort(trackletGroup[i].begin(),trackletGroup[i].end());	// sort by index of tracklet

		Tracklet *newTracklet = new Tracklet();		
		vector<int> temp = trackletGroup[i];		
		for (int j = 1; j < temp.size(); j++)
		{			
			tracklets[temp[0]].trackletSet.insert(tracklets[temp[0]].trackletSet.end(), 
				tracklets[temp[j]].trackletSet.begin(),tracklets[temp[j]].trackletSet.end());

			tracklets[temp[0]].frame.insert(tracklets[temp[0]].frame.end(), 
				tracklets[temp[j]].frame.begin(),tracklets[temp[j]].frame.end());

			trackletToRemove.push_back(temp[j]);
		}
		//tracklets[temp[0]].label = label;
		//tracklets[temp[0]].discrmSet.clear();

		// update discriminative set
		//for (i = 0; i < label; i++)
		//{
		//	tracklets[i].discrmSet.push_back(label);
		//}
		//label++;
	}

	printf("remove redundant tracklets\n");
	// remove redundant tracklets
	label = 0;
	for (int i = 0; i < tracklets.size(); i++)
	{
		if (find(trackletToRemove.begin(), trackletToRemove.end(), i) == trackletToRemove.end())
		{			
			tracklets[i].discrmSet.clear(); // reset discriminative set
			//tracklets[i].label = label;
			newTracklets.push_back(tracklets[i]);

			// update discriminative set
			for (i = 0; i < label; i++)
			{
				newTracklets[i].discrmSet.push_back(label);
				newTracklets[label].discrmSet.push_back(i);
			}
			label++;
		}		
	}
	
	//printf("release mem\n");
	container.tracklets.clear();

	//printf("release mem1\n");
	container.tracklets = newTracklets;


	trackletGroup.clear();
	trackletToRemove.clear();
}

/************************************************************************/
/* Function to check if a tracklet have already in tracketGroup         */
/************************************************************************/
int Sample::isTraversed(int keyword, vector<vector<int>> trackletGroup)
{	
	for (int i = 0; i < trackletGroup.size(); i++)
	{
		vector<int> temp = trackletGroup[i];
		for (int j = 0; j < temp.size(); j++)
		{
			if (keyword == temp[j])
			{
				return i;	// return the group position
			}
		}
	}
	return -1;
}

void Sample::NextProcess(int ignoreFrame)
{
	container.PrepareNextProcess(ignoreFrame);
	positiveSmp.clear();
	negativeSmp.clear();
	featurePool.clear();
	strongClassifier.clear();
	yi.clear();
	index.clear();
	output.clear();
	elem.clear();
	alpha.clear();
}

void Sample::NormalizeH()
{
	float maxVal = *std::max_element(output.begin(), output.end());
	float minVal = *std::min_element(output.begin(), output.end());

	for (int i = 0; i < output.size(); i++)
	{
		output[i] = Utils::map(output[i], minVal, maxVal, 0, 1);
	}
}